# 作者：hehung
# 联系：1398660197@qq.com
# 微信：hehung95

import socket
import sys
import cv2
import numpy as np
import sys
from threading import Thread
import struct
import queue
# import time
import serial

# IP地址以及端口
SERVER_IP_PORT_SEND = ('192.168.6.101', 6789)
SERVER_IP_PORT_RECV = ('192.168.6.105', 6790)
CHUNK_SIZE = 8192

steer = [7.5, 7.5, 7.5, 7.5, 7.5, 7.5]

# 打开串口连接
target_serial = serial.Serial('/dev/ttyS1', 115200)

class ImgClass:
    def __init__(self):
        # 获取摄像机
        self.cap = cv2.VideoCapture(0)

    def read_cap(self):
        # 读取摄像头采集的视频
        ret, frame = self.cap.read()
        return frame

    def show_img(self, img):
        # 显示图片
        # 放大图片到原始尺寸
        frame = cv2.resize(img, (1080, 810))  
        cv2.imshow('Capture', frame)
        c = cv2.waitKey(1)
        # 如果按下ESC则退出
        if c == 27:
            self.close_img()
            return 0
        else:
            return 1

    # 关闭显示
    def close_img(self):
        self.cap.release()
        cv2.destroyAllWindows()

    # 获取图片大小
    def get_img_size(self, img):
        height, width = img.shape[:2]

        return [height, width]

    # 将图片转换字节
    def img_2_bytes(self, img):
        img_encode = cv2.imencode('.jpg', img)[1]
        data_encode = np.array(img_encode)
        data = data_encode.tobytes()
        return data
    
    # 将字节转换为图片
    def bytes_2_img(self, bytes):
        nparr = np.frombuffer(bytes, np.uint8)
        img_decode = cv2.imdecode(nparr, flags=cv2.IMREAD_COLOR)
        return img_decode

class SocketClass:
    def __init__(self, ip_port):
        try:
            # 创建一个基于 IPv4 和 UDP 协议的套接字对象
            self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            self.client_socket.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 650000)
            self.client_socket.setsockopt(socket.SOL_SOCKET, socket.SO_SNDBUF, 650000)
            # 连接到指定的服务器主机和端口
            # self.client_socket.connect(ip_port)
            self.server_ip_port = ip_port
            print (self.client_socket.getsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF))
        except socket.error as msg:
            print(msg)
            print(sys.exit(1))

    # 绑定IP和端口
    def socket_bind(self):
        # 将套接字绑定到指定的主机和端口
        self.client_socket.bind(self.server_ip_port)

    # 分包发送图片
    def send_in_chunks(self, img_bytes):
        # 发送图片
        self.client_socket.sendto(img_bytes, self.server_ip_port)


    # 分包接收图片
    def recv_in_chunks(self):
        data_buffer, addr = self.client_socket.recvfrom(400000)
        steer[0], steer[1], steer[2], steer[3], steer[4], steer[5] = struct.unpack("!6f", data_buffer[:24])
        print (steer[0], steer[1], steer[2], steer[3], steer[4], steer[5])
        img_data = data_buffer[24:]
        return img_data

def uart_send():
    data_str = 'START::0-%.2f:1-%.2f:2-%.2f:3-%.2f:4-%.2f:5-%.2f::END\r\n' % (steer[0], steer[1], steer[2], steer[3], steer[4], steer[5])
    print ('Send Data: %s' % data_str.encode())
    target_serial.write(data_str.encode())

def img_recv_task():
    while True:
        # 接收来自UDP的数据
        img_data = udp_socket_recv.recv_in_chunks()
        # 转换为图片数组
        recv_img = img_handle.bytes_2_img(img_data)
        uart_send()
        recv_queue.put(recv_img)

if __name__ == '__main__':
    # 创建一个队列
    recv_queue = queue.Queue(maxsize=50)
    print ('Start Client')
    img_handle = ImgClass()
    udp_socket_recv = SocketClass(SERVER_IP_PORT_RECV)
    udp_socket_recv.socket_bind()
    udp_socket_send = SocketClass(SERVER_IP_PORT_SEND)
    # 创建线程对象
    recv_t = Thread(target=img_recv_task)
    # 设置为守护进程
    recv_t.setDaemon(True)
    # 启动线程
    recv_t.start()
    print ('Client Started')
    # 判断串口是否成功打开
    if target_serial.isOpen():                       
        print("Open serial success")
        print(target_serial.name)    # 输出串口号
    else:
        print("Open serial fail")

    while True:
        # 采集摄像机图像并返回图片数据
        frame = img_handle.read_cap()
        frame = cv2.flip(frame, 1)
        # 缩小画面，方便传输
        frame = cv2.resize(frame,(400, 300))  
        # 将图片数据转换为字节方便发送给服务器
        frame_data = img_handle.img_2_bytes(frame)
        # 将图片发送到服务器
        udp_socket_send.send_in_chunks(frame_data)

        # 从服务器接收图片数据
        if recv_queue.qsize() != 0:
            # print (print(recv_queue.qsize()))
            frame = recv_queue.get()
            # 显示图像
            if (0 == img_handle.show_img(frame)):
                break
        # time.sleep(0.001)
    print ('Exit by manual')
